#include <QtGui>
#include "mathbox.h"

MathBox::MathBox()
: dt(0.1)
, timerID(-1)
, deb(0)
, broaching(before)
{
	shipModel.length = 26.0;
	shipModel.width = 9.0;
	shipModel.displacement = 170000.0;
	shipModel.draught = 1.9;
	shipModel.Xp = 0.0;
	shipModel.R = 0.0;
	shipModel.q = 0.0;
	shipModel.F = 50.0;
	shipModel.mass = RHO * shipModel.displacement;
	shipModel.Jz = shipModel.mass * (shipModel.length * shipModel.length + shipModel.width * shipModel.width) / 12.0;

	waterModel.wavePeriod = 4.0;
	double waveAmplitude = waterModel.wavePeriod * waterModel.wavePeriod / 46.08;
	waterModel.a = waveAmplitude * 2 / sqrt(5.0);
	waterModel.b = 0.5 * waterModel.a;
	
	waterModel.waveFrequency = 2.0 * M_PI / waterModel.wavePeriod;

	waterModel.K = -waterModel.waveFrequency * waterModel.waveFrequency / G;

	waterModel.waveAngleAmplitude = sqrt(waterModel.a*waterModel.a*waterModel.K*waterModel.K + waterModel.b*waterModel.b*waterModel.K*waterModel.K);
	waterModel.waveVelocity = G / waterModel.waveFrequency;
	waterModel.waveLength = G * waterModel.wavePeriod * waterModel.wavePeriod / 2.0 / M_PI;

	shipModel.course = 0.0;
	shipModel.velocity = 0.001*waterModel.waveVelocity;
	shipModel.angleVelocity = 0.0;
	shipModel.drift = 0.0;
	shipModel.dRollAngle = shipModel.rollAngle = 0.0;
	shipModel.heave = shipModel.dHeave = 0.0;

	shipModel.ownRollPeriod = 2*M_PI*sqrt((shipModel.width*shipModel.width+4.0*1.0*1.0)/10/G/0.5);
	shipModel.rollNi = 0.01/M_PI*shipModel.ownRollPeriod;

	shipModel.ownHeavePeriod = 2.0;
	shipModel.heavNi = 0.04;


	shipModel.prev = shipModel.preprev = QPointF(0.0, 0.0);



	m11 = 2.0 * shipModel.mass / (RHO * shipModel.length * shipModel.length * shipModel.draught);
	m22 = m11 + M_PI * shipModel.draught / waterModel.waveFrequency / shipModel.length;
	m66 = 2.0 * shipModel.Jz / (RHO * waterModel.waveFrequency * shipModel.length * shipModel.length * shipModel.length * shipModel.draught)
		+ shipModel.draught * M_PI / (12.0 * waterModel.waveFrequency * shipModel.length);

	emit MathModelChanged(shipRenderData);
}

MathBox::~MathBox()
{

}

void MathBox::calc()
{
	double ds = shipModel.velocity * dt / shipModel.length;
	double shipVelDimless = shipModel.velocity / waterModel.waveVelocity;
	double shipAngleVelDimless = shipModel.angleVelocity * shipModel.length / shipModel.velocity;

	if(broaching == before)
	{
		if (shipRenderData.timeStamp >= waterModel.wavePeriod*5)
			shipVelDimless += 0.01;
	} else if(broaching == inProgress)
	{
		double k1 = VelocityFunction(shipVelDimless, shipModel.drift, shipAngleVelDimless, shipModel.q, shipModel.course) * ds;
		double m1 = DriftFunction(shipVelDimless, shipModel.drift, shipAngleVelDimless, shipModel.q, shipModel.course) * ds;
		double n1 = AngleVelFunction(shipVelDimless, shipModel.drift, shipAngleVelDimless, shipModel.q, shipModel.course) * ds;
		double o1 = QFunction(shipVelDimless, shipModel.drift, shipModel.course) * ds;
		double p1 = CourseFunction(shipAngleVelDimless) * ds;

		double k2 = VelocityFunction(shipVelDimless + k1/2.0, shipModel.drift + m1/2.0, shipAngleVelDimless + n1/2.0, shipModel.q + o1/2.0, shipModel.course + p1/2.0) * ds;
		double m2 = DriftFunction(shipVelDimless + k1/2.0, shipModel.drift + m1/2.0, shipAngleVelDimless + n1/2.0, shipModel.q + o1/2.0, shipModel.course + p1/2.0) * ds;
		double n2 = AngleVelFunction(shipVelDimless + k1/2.0, shipModel.drift + m1/2.0, shipAngleVelDimless + n1/2.0, shipModel.q + o1/2.0, shipModel.course + p1/2.0) * ds;
		double o2 = QFunction(shipVelDimless + k1/2.0, shipModel.drift + m1/2.0, shipModel.course + p1/2.0) * ds;
		double p2 = CourseFunction(shipAngleVelDimless + n1/2.0) * ds;

		double k3 = VelocityFunction(shipVelDimless + k2/2.0, shipModel.drift + m2/2.0, shipAngleVelDimless + n2/2.0, shipModel.q + o2/2.0, shipModel.course + p2/2.0) * ds;
		double m3 = DriftFunction(shipVelDimless + k2/2.0, shipModel.drift + m2/2.0, shipAngleVelDimless + n2/2.0, shipModel.q + o2/2.0, shipModel.course + p2/2.0) * ds;
		double n3 = AngleVelFunction(shipVelDimless + k2/2.0, shipModel.drift + m2/2.0, shipAngleVelDimless + n2/2.0, shipModel.q + o2/2.0, shipModel.course + p2/2.0) * ds;
		double o3 = QFunction(shipVelDimless + k2/2.0, shipModel.drift + m2/2.0, shipModel.course + p2/2.0) * ds;
		double p3 = CourseFunction(shipAngleVelDimless + n2/2.0) * ds;

		double k4 = VelocityFunction(shipVelDimless + k3, shipModel.drift + m3, shipAngleVelDimless + n3, shipModel.q + o3, shipModel.course + p3) * ds;
		double m4 = DriftFunction(shipVelDimless + k3, shipModel.drift + m3, shipAngleVelDimless + n3, shipModel.q + o3, shipModel.course + p3) * ds;
		double n4 = AngleVelFunction(shipVelDimless + k3, shipModel.drift + m3, shipAngleVelDimless + n3, shipModel.q + o3, shipModel.course + p3) * ds;
		double o4 = QFunction(shipVelDimless + k3, shipModel.drift + m3, shipModel.course + p3) * ds;
		double p4 = CourseFunction(shipAngleVelDimless + n3) * ds;

		shipVelDimless += (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;
		shipModel.drift += (m1 + 2.0 * m2 + 2.0 * m3 + m4) / 6.0;
		shipAngleVelDimless += (n1 + 2.0 * n2 + 2.0 * n3 + n4) / 6.0;
		shipModel.q += (o1 + 2.0 * o2 + 2.0 * o3 + o4) / 6.0;
		shipModel.course += (p1 + 2.0 * p2 + 2.0 * p3 + p4) / 6.0;
	} else
	{
		shipAngleVelDimless = 0.0;

		shipModel.drift *= 0.95;

		if(fabs(shipModel.drift) < 1e-4)
			shipModel.drift = 0.0;
	}

	if(shipVelDimless >= 0.9 && broaching == before)
	{
		shipVelDimless = 0.9;
		broaching = inProgress;
		shipModel.drift = 0.01;
		shipAngleVelDimless = -0.14;
	}


	if((shipModel.course < -1.57 || shipModel.course > 1.57) &&  broaching == inProgress)
		broaching = happened;

	shipModel.velocity = shipVelDimless * waterModel.waveVelocity;
	shipModel.angleVelocity = shipAngleVelDimless * shipModel.velocity / shipModel.length;
	
	shipModel.course += shipModel.angleVelocity * dt;

	shipModel.preprev = shipModel.prev;
	shipModel.prev = QPointF(shipRenderData.x, shipRenderData.y);

	shipRenderData.x += shipModel.velocity * dt * cos(shipModel.course - shipModel.drift);
	shipRenderData.y += shipModel.velocity * dt * sin(shipModel.course - shipModel.drift);
	shipRenderData.course = shipModel.course * RAD2DEG;

	emit setGraphicValue(GraphicData(shipRenderData.timeStamp, shipAngleVelDimless, shipVelDimless, shipModel.drift));

	double r1 = RollFunction(shipModel.rollAngle, shipModel.dRollAngle, shipRenderData.timeStamp) * dt;
    double r2 = RollFunction(shipModel.rollAngle + shipModel.dRollAngle * dt / 2.0, shipModel.dRollAngle + r1 / 2.0 ,shipRenderData.timeStamp + dt / 2.0) * dt;
    double r3 = RollFunction(shipModel.rollAngle + shipModel.dRollAngle * dt / 2.0 + r1 * dt / 4.0, shipModel.dRollAngle + r2 / 2.0, shipRenderData.timeStamp + dt / 2.0) * dt;
    double r4 = RollFunction(shipModel.rollAngle + shipModel.dRollAngle * dt + r2 * dt / 2.0, shipModel.dRollAngle + r3, shipRenderData.timeStamp + dt) * dt;
    
    shipModel.rollAngle += shipModel.dRollAngle * dt + (r1 + r2 + r3) * dt / 6.0;
    shipModel.dRollAngle += (r1 + 2.0 * r2 + 2.0 * r3 + r4) / 6.0;

	shipRenderData.rollAngle = shipModel.rollAngle * RAD2DEG;

	double h1 = HeaveFunction(shipModel.heave, shipModel.dHeave, shipRenderData.timeStamp) * dt;
	double h2 = HeaveFunction(shipModel.heave + shipModel.dHeave * dt / 2.0, shipModel.dHeave + h1 / 2.0 ,shipRenderData.timeStamp + dt / 2.0) * dt;
	double h3 = HeaveFunction(shipModel.heave + shipModel.dHeave * dt / 2.0 + h1 * dt / 4.0, shipModel.dHeave + h2 / 2.0, shipRenderData.timeStamp + dt / 2.0) * dt;
    double h4 = HeaveFunction(shipModel.heave + shipModel.dHeave * dt + h2 * dt / 2.0, shipModel.dHeave + h3, shipRenderData.timeStamp + dt) * dt;

	shipModel.heave += shipModel.dHeave * dt + (h1 + h2 + h3) * dt / 6.0;
    shipModel.dHeave += (h1 + 2.0 * h2 + 2.0 * h3 + h4) / 6.0;

	shipRenderData.heaving = shipModel.heave;

	shipRenderData.timeStamp += dt;

	emit setGraphicValue(GraphicData(shipRenderData.timeStamp, shipModel.angleVelocity * shipModel.length / shipModel.velocity,
		shipModel.velocity / waterModel.waveVelocity, shipModel.drift));
}

void MathBox::timerEvent(QTimerEvent *e)
{
	calc();
	emit MathModelChanged(shipRenderData);
}

void MathBox::setPlay()
{
	if(timerID < 0)
		timerID = startTimer(50);
	else
	{
		killTimer(timerID);
		timerID = -1;
	}
}

double MathBox::ForceX(const double shipVelDimless, const double q)
{
	double ForceDist = 4.0 * shipModel.width / waterModel.waveFrequency / shipModel.length * waterModel.waveAngleAmplitude;
	double ForceAero = 0.0;
	return 2.0 * (shipModel.Xp - shipModel.R) / (RHO * waterModel.waveVelocity*waterModel.waveVelocity*shipVelDimless*shipModel.F)
		+ ForceDist * sin(q) / shipVelDimless / shipVelDimless
		+ ForceAero;
}

double MathBox::ForceY(const double shipVelDimless, const double q, const double shipCourse)
{
	double Cyh = 1.0;
	double ForceDist = 4.0 * shipModel.width / waterModel.waveFrequency
		/ shipModel.length * waterModel.waveAngleAmplitude * tan(shipCourse)
		+ M_PI * shipModel.draught * sin(shipCourse) / shipModel.width * (1.0 + shipVelDimless * cos(shipCourse) / 2.0);

	double ForceAero = 0.0;

	return Cyh - ForceDist / shipVelDimless / shipVelDimless * sin(q) + ForceAero;
}

double MathBox::TorgueZ(const double shipVelDimless, const double q, const double shipCourse)
{
	double Mzh = 4.0;
	double TorgueDist = M_PI * shipModel.width / 2.0 / waterModel.waveFrequency / waterModel.waveLength
		*(M_PI*shipModel.draught/shipModel.width *(1-shipVelDimless*cos(shipCourse))
		+2*shipModel.draught/M_PI/shipModel.width*pow(waterModel.waveLength/shipModel.length,2)*shipVelDimless/cos(shipCourse))*sin(shipCourse);
	double TorgueAero = 0.0;

	return Mzh + TorgueDist / shipVelDimless / shipVelDimless * cos(q) + TorgueAero;
}

double MathBox::DriftFunction(const double shipVelDimless, const double shipDrift,
							  const double shipAngleVelDimless, const double q, const double shipCourse)
{
	double forceX = ForceX(shipVelDimless, q);
	double forceY = ForceY(shipVelDimless, q, shipCourse);
	return shipAngleVelDimless * (pow(cos(shipDrift),2) * m11 / m22 + pow(sin(shipDrift),2) * m22 / m11)
		- forceY * cos(shipDrift) / m22 - forceX * sin(shipDrift) / m11;
}

double MathBox::VelocityFunction(const double shipVelDimless, const double shipDrift,
								 const double shipAngleVelDimless, const double q, const double shipCourse)
{
	double forceX = ForceX(shipVelDimless, q);

	double driftDer = DriftFunction(shipVelDimless, shipDrift, shipAngleVelDimless, q, shipCourse);

	return shipVelDimless * tan(shipDrift) * driftDer
		+ shipVelDimless * (forceX - m22 * sin(shipDrift) * shipAngleVelDimless) / (m11 * cos(driftDer));
}

double MathBox::AngleVelFunction(const double shipVelDimless, const double shipDrift,
						const double shipAngleVelDimless, const double q, const double shipCourse)
{
	double torgueZ = TorgueZ(shipVelDimless, q, shipCourse);

	double velDer = VelocityFunction(shipVelDimless, shipDrift, shipAngleVelDimless, q, shipCourse);

	return torgueZ /m66 - shipAngleVelDimless/shipVelDimless * velDer;
}

double MathBox::QFunction(const double shipVelDimless, const double shipDrift, const double shipCourse)
{
	return 2*M_PI / waterModel.waveLength * (1/shipVelDimless - cos(shipCourse - shipDrift));
}

double MathBox::CourseFunction(const double shipAngleVelDimless)
{
	return shipAngleVelDimless;
}

double MathBox::RollFunction(const double rollAngle, const double drollAngle, const double t)
{
	QPointF current(shipRenderData.x,shipRenderData.y);
	QLineF a(shipModel.preprev, current);
	QLineF b(shipModel.prev, current);
	QLineF c(shipModel.preprev, shipModel.prev);

	double rad = 0.0;
	double circMoment = 0.0;
	if(circRad(a.length(), b.length(), c.length(), rad))
		circMoment = 0.0000000005*shipModel.mass*shipModel.velocity*shipModel.velocity/rad;

	double circVectorAngle = atan((current.y()-shipModel.preprev.y()) / (current.x()-shipModel.preprev.x()));
	if(circVectorAngle > shipModel.course)
		circMoment = -circMoment;	

	double n = 2*M_PI / shipModel.ownRollPeriod;

	return n*n*waterModel.waveAngle(shipRenderData.x,shipRenderData.y,shipModel.course,t) + circMoment
		-n*n*rollAngle - 2*shipModel.rollNi*drollAngle;
}

double MathBox::HeaveFunction(const double heave, const double dHeave, const double t)
{
	double height = waterModel.waveFunction(shipRenderData.x,shipRenderData.y,t);
	double n = 2*M_PI / shipModel.ownHeavePeriod;
	return n * n * height - 2 * shipModel.heavNi * dHeave - n*n*heave;
}

bool MathBox::circRad(const double a, const double b, const double c, double& r)
{
	double S = sqrt((a+b+c)*(b+c-a)*(a+c-b)*(a+b-c))/4;
	if(S <=1e-10)
		return false;
	else
		r =  a*b*c/4/S;
	return true;
}

double MathBox::angleveldim(double time)
{
	double t = time - deb;

	double ret = pow(t,6)*0.01858-pow(t,5)*0.167926+pow(t,4)*0.547197-pow(t,3)*0.841112+pow(t,2)*0.668376-t*0.067598+0.053022;

	/*if(t>4)
		ret = -pow(t,3)*0.163226+pow(t,2)*2.718867-15.032508*t+27.575044;*/

	if(t>4.9)
		ret = 0.0;

	return ret;
}

double MathBox::linearveldim(double time)
{
	double t = time - deb;

	double ret = 1.0;
	if(t > 2.813083994 && t < 4.3)
		ret = pow(t,3)*0.487939-pow(t,2)*4.766223+t*15.07681-14.559227;

	return ret;
}

double MathBox::driftangle(double time)
{
	double t = time - deb;

	double ret = t >= 3.682242836 ? -pow(t,3)*25.838152+pow(t,2)*297.136754-t*1137.128281+1448.209394 : -pow(t,5)*0.025407+pow(t,4)*0.176184-pow(t,3)*0.410151+pow(t,2)*0.411022-t*0.117525+0.00221;
	if(t > 4)
		ret = -pow(t,3)*0.984128+pow(t,2)*13.534892-t*62.006797+94.677691;
	if(t > 5)
		ret = 0.0;

	return ret;
}